from maix import image, camera, display, app, time , uart, pinmap , gpio
import numpy as np 
import cv2 , struct

kernel = cv2.getStructuringElement(cv2.MORPH_RECT,(3,3))
cam = camera.Camera(320,240) #初始化相机
disp = display.Display() #初始化显示屏

device1 = "/dev/ttyS0"          # 示例的串口名称                        此串口设备名称为串口0
uart1 = uart.UART(device1 , 115200)
pinmap.set_pin_function("A29","GPIOA29")
led = gpio.GPIO("GPIOA29",gpio.Mode.OUT)
led.value(0)

def get_contour(flag=0):
    points = []
    back_area = 0
    point_x = 5000
    point_y = 5000
    cv_img_raw = image.image2cv(img) # 转cv
    cv_img = cv2.cvtColor(cv_img_raw,cv2.COLOR_BGR2GRAY) # 转灰度
    #cv_img = cv2.blur(cv_img,(3,3))
    #cv_img = cv2.bilateralFilter(cv_img,9,10,10)     # 双边滤波
    #cv_img = cv2.morphologyEx(cv_img,cv2.MORPH_CLOSE,kernel) # 闭运算
    cv_img = cv2.Canny(cv_img ,150,300) # 边缘检测
    #cv_img = cv2.morphologyEx(cv_img, cv2.MORPH_CLOSE, np.ones((7,7), np.uint8))  # 闭合大裂缝
    #cv_img = cv2.dilate(cv_img, np.ones((3,3), np.uint8), iterations=1)          # 加粗边缘
    if flag:
        img_show = image.cv2image(cv_img)
        disp.show(img_show)
        print(111)
    else:
        contours , _ = cv2.findContours(cv_img,cv2.RETR_EXTERNAL,cv2.CHAIN_APPROX_SIMPLE) # 找轮廓
        for contour in contours:
            area = cv2.contourArea(contour)
            if area < 1000:
                continue
            
            epsilon  = 0.02 * cv2.arcLength(contour,True)  # 多边形逼近
            approx = cv2.approxPolyDP(contour,epsilon,True)
            if len(approx) == 4:
                back_area = area
                M = cv2.moments(contour)
                point_x = int(M["m10"]/M["m00"])
                point_y = int(M["m01"]/M["m00"])
                cv2.circle(cv_img_raw , ( point_x , point_y ) , 2 , ( 255 , 0 , 0 ) , 1)
                cv2.drawContours(cv_img_raw,[approx],0,(0,255,0),2)
                quad_points = []
                for point in approx:
                    x , y = point.ravel()
                    quad_points.append((x, y))  # 收集当前四边形的点
                    #cv2.circle(cv_img_raw , ( x , y ) , 2 , ( 255 , 0 , 0 ) , 1)
                
                points = quad_points
        img_show = image.cv2image(cv_img_raw)

    return img_show , points ,back_area


"""
    根据矩形的四个角点坐标计算中心点
    参数:
        points: 包含四个元组的列表，每个元组表示一个角点的(x, y)坐标，顺序不固定
    返回:
        中心点坐标(center_x, center_y)
    """
def calculate_rectangle_center(points):
    
    if len(points) != 4:
        print("需要提供4个角点坐标")
        return 0 ,0
    
    # 提取所有x坐标和y坐标
    x_coords = [p[0] for p in points]
    y_coords = [p[1] for p in points]
    
    # 计算中心点坐标
    center_x = sum(x_coords) / 4
    center_y = sum(y_coords) / 4
    return center_x, center_y

def uart_send(serial:uart.UART,head,data):
    send_data = struct.pack(f"<B{len(data)}h",head,*data)
    serial.write(send_data)

def sort_rotated_rect(points):
    """
    对旋转矩形的四个点排序：左上→左下→右下→右上
    :param points: 形状为(4,2)的NumPy数组
    :return: 排序后的点
    """
    # 计算中心点
    points = np.array(points, dtype=np.uint16)  # 显式转换
    center = np.mean(points, axis=0)
    
    # 按极角排序（顺时针）
    angles = np.arctan2(points[:, 1] - center[1], points[:, 0] - center[0])
    sorted_indices = np.argsort(angles)
    sorted_points = points[sorted_indices]
    
    # 调整起点：左上角（y值最小的点）
    top_left_index = np.argmin(sorted_points[:, 1])
    ordered_points = np.roll(sorted_points, -top_left_index, axis=0)
    
    return ordered_points

def deal_center(points,threshold):
    if len(points) == 4:
        points = sort_rotated_rect(points)
        left_len = points[2][1] - points[3][1]
        right_len = points[1][1] - points[0][1]
        
        if right_len > left_len :
            print("在右边")
            if 5000 < threshold < 20000:
                print(111)
                return int(-14)
            else :
                print(222)
                return int(-27)
            
        else:
            print("在左边")
            if 5000 < threshold < 20000:
                return int(-23)
            else :
                return int(-27)
            
    
    else:
        return 0


k = 2    # 矫正系数
first_correct = 18
correct_area = 11600



while not app.need_exit():
    img = cam.read()
    ex , ey = 0 , 0
    area = 0

    center_x = cam.width() / 2 + first_correct
    center_y = cam.height()/ 2

    img , points, area = get_contour(flag = 0)
    
    #rint(points)
    if area != 0:
        correct = k * ( area / correct_area)
        center_x = center_x  + correct
        deal = deal_center(points,area)
        center_x = int(center_x) + deal
        c_x , c_y = calculate_rectangle_center(points)
        c_x = int(c_x)
        c_y = int(c_y)
        ex = c_x - center_x
        ey = c_y - center_y
        print(area)

    else :
        print("没有识别")
        

    
    uart_send(uart1,0xaa,[int(ex),-int(ey)])
    img.draw_keypoints([int(center_x),int(center_y)],color=image.COLOR_BLUE)
    img.draw_string(0,0,f"fps is {time.fps()}",color=image.COLOR_BLUE)
    img.draw_string(0,40,f"ex is {ex}",color=image.COLOR_BLUE)
    img.draw_string(0,80,f"ey is {-ey}",color=image.COLOR_BLUE)
    # if(ex <= 3 and ey <= 3):
    #     led.value(0)
    # else:
    #     led.value(0)
    disp.show(img)
